#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/Int32.h>
#include <math.h>

/// This node publishes all joint_states, including the angle of the kinect on the arduino board

sensor_msgs::JointState js_;

double angle;

void angleCallback(const std_msgs::Int32::ConstPtr& msg)
{
   angle = (msg->data)*M_PI/180; // angle in rad, msg in degree
}




int main(int argc, char** argv){

  ros::init(argc, argv, "JointState");

  ros::NodeHandle n;

  angle = 0;

  ros::Publisher joint_state_pub = n.advertise<sensor_msgs::JointState>("/joint_states", 1);

  ros::Subscriber angle_sub = n.subscribe("/cmd_angle", 10, angleCallback);

  js_.name.push_back("base_arduino_platform_joint" );
  js_.position.push_back(0);
  js_.velocity.push_back(0);
  js_.effort.push_back(0);

  js_.name.push_back("wheel_rf_rot_z_joint");
  js_.position.push_back(0);
  js_.velocity.push_back(0);
  js_.effort.push_back(0);

  js_.name.push_back("wheel_lb_rot_z_joint");
  js_.position.push_back(0);
  js_.velocity.push_back(0);
  js_.effort.push_back(0);

  js_.name.push_back("wheel_lf_rot_z_joint");
  js_.position.push_back(0);
  js_.velocity.push_back(0);
  js_.effort.push_back(0);

  js_.name.push_back("wheel_rb_rot_z_joint" );
  js_.position.push_back(0);
  js_.velocity.push_back(0);
  js_.effort.push_back(0);

  js_.name.push_back("wheel_rf_rot_y_joint");
  js_.position.push_back(0);
  js_.velocity.push_back(0);
  js_.effort.push_back(0);

  js_.name.push_back("wheel_lb_rot_y_joint" );
  js_.position.push_back(0);
  js_.velocity.push_back(0);
  js_.effort.push_back(0);

  js_.name.push_back("wheel_lf_rot_y_joint" );
  js_.position.push_back(0);
  js_.velocity.push_back(0);
  js_.effort.push_back(0);

  js_.name.push_back("wheel_rb_rot_y_joint");
  js_.position.push_back(0);
  js_.velocity.push_back(0);
  js_.effort.push_back(0);



  ros::Rate r(5.0);
  while(n.ok()){
      ros::spinOnce();
      ros::Time current_time = ros::Time::now();

      js_.header.stamp.sec = current_time.sec;
      js_.header.stamp.nsec = current_time.nsec;

      js_.position[0] = angle;

      joint_state_pub.publish( js_ );

      r.sleep();
  }
}

